#ifndef _INCL_CLASERSENSOR
#define _INCL_CLASERSENSOR

#include <mrpt/poses/CPose3D.h>
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>
#include <mrpt/utils.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
#include <mrpt/gui/WxSubsystem.h>
#include <mrpt/gui/WxUtils.h>

#define LASER_MAX_RANGE 5.6

using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::math;
using namespace mrpt::utils;

using namespace std;

class CLaserSensor 
{
     public : 
     
     CLaserSensor();
     ~CLaserSensor();
     
      
      
     CPose3D laser_pose;
     
     CObservation2DRangeScan scan_obs;
     
    CPointCloudPtr point_cloud;
     
     void setPose(CPose3D pose);
     
    void simulateLaserScan(COpenGLViewportPtr view ); 
    
    bool scanBox(CBoxPtr box,TPoint3D A,TPoint3D B,TPoint3D& inter_pt,double& range);
    
    bool scanFace(TPoint3D P1,TPoint3D P2,TPoint3D P3,TPoint3D P4,TPoint3D A,TPoint3D B,TPoint3D& inter_pt,double& range);
    
    bool scanGridPlaneXY(CGridPlaneXYPtr plane,TPoint3D A,TPoint3D B,TPoint3D& inter_pt,double& range);
    
    bool scanCylinder(CCylinderPtr cyl,TPoint3D A,TPoint3D B,TPoint3D& inter_pt,double& range);
    
    double scalarProd(TPoint3D p1,TPoint3D p2);
   
};


#endif